﻿using System;
using System.Collections.Generic;
using System.Text;
using BarStructures.Deformed;
using MathNet.Numerics.LinearAlgebra;
using SimpleLinearAlgebra;
using MathNet.Numerics;

namespace BarStructures.Analysers
{
    /// <summary>
    /// Рама.
    /// Заполненная матрица масс с учётом инерции вращения узлов.
    /// Степени свободы узлов: x, y, rotation.
    /// </summary>
    public class FrameAnalyser_XYR_CRM: Analyser
    {
        private Matrix eigenVectors = null;

        private Vector eigenValues = null;

        private bool massWithRotationInertia;

        public FrameAnalyser_XYR_CRM(Model model, string name, bool massWithRotationInertia)
            :base(model, name)
        {
            this.massWithRotationInertia = massWithRotationInertia;
        }

        public override void Analyse()
        {
            DeformedState ds = new FrameDeformedState();
            ds.AssignToModel(model);

            if (!UseExistingIndexation)
                model.SetAutoSystemIndexes();

            int modelDimension = model.GetNodesCount() * 3;

            //подготовка матриц
            Matrix _stiffness = new Matrix(modelDimension, modelDimension);
            Matrix _mass = new Matrix(modelDimension, modelDimension);
            SimpleMatrix stiffness = new SimpleMatrix(modelDimension, modelDimension);
            SimpleMatrix mass = new SimpleMatrix(modelDimension, modelDimension);

            //построение матриц жесткости и масс
            Matrix _elementMatrix;
            ISimpleMatrix elementMatrix;
            int barsCount, ind1, ind2;
            barsCount = model.GetBarsCount();
            for (int i = 0; i < barsCount; i++)
            {
                Bar bar = model.GetBar(i);
                ind1 = bar.NodeOne.SystemIndex * 3;
                ind2 = bar.NodeTwo.SystemIndex * 3;

                elementMatrix = BarGlobalStiffnessMatrix(bar);
                stiffness.AddSubMatrix(ind1, ind1, elementMatrix.GetSubMatrix(0, 0, 3, 3));
                stiffness.AddSubMatrix(ind1, ind2, elementMatrix.GetSubMatrix(0, 3, 3, 3));
                stiffness.AddSubMatrix(ind2, ind1, elementMatrix.GetSubMatrix(3, 0, 3, 3));
                stiffness.AddSubMatrix(ind2, ind2, elementMatrix.GetSubMatrix(3, 3, 3, 3));

                if (massWithRotationInertia)
                    elementMatrix = BarGlobalMassMatrixWithRotationInertia(bar);
                else
                    elementMatrix = BarGlobalMassMatrix(bar);
                mass.AddSubMatrix(ind1, ind1, elementMatrix.GetSubMatrix(0, 0, 3, 3));
                mass.AddSubMatrix(ind1, ind2, elementMatrix.GetSubMatrix(0, 3, 3, 3));
                mass.AddSubMatrix(ind2, ind1, elementMatrix.GetSubMatrix(3, 0, 3, 3));
                mass.AddSubMatrix(ind2, ind2, elementMatrix.GetSubMatrix(3, 3, 3, 3));
            }

            //исключение связанных степеней свободы
            int[] indexes = new int[modelDimension];
            int counter = 0;
            for (int i = 0; i < model.GetNodesCount(); i++)
            {
                Node node = model.GetNodeBySystemIndex(i);
                if (!node.Support.Sx)
                    indexes[counter++] = i * 3;
                if (!node.Support.Sy)
                    indexes[counter++] = i * 3 + 1;
                if (!node.Support.Sr)
                    indexes[counter++] = i * 3 + 2;
            }
            Array.Resize<int>(ref indexes, counter);

            stiffness = (SimpleMatrix)stiffness.GetSubMatrix(indexes, indexes);
            mass = (SimpleMatrix)mass.GetSubMatrix(indexes, indexes);

            //вычисление собственных значений и собственных векторов
            Matrix ir_stiffness = new Matrix(stiffness.GetInternalData());
            Matrix ir_mass = new Matrix(mass.GetInternalData());
            Matrix ir = ir_mass.Inverse().Multiply(ir_stiffness);

            EigenvalueDecomposition evd = new EigenvalueDecomposition(ir);
            eigenVectors = evd.EigenVectors;
            eigenValues = evd.RealEigenvalues;
        }

        public override DeformedState GetDeformedStateForEigenValue(double eigenValue)
        {
            if (eigenValues == null)
                Analyse();

            int i = 0;
            while (i < eigenValues.Length && !Number.AlmostEqual(eigenValue, eigenValues[i]))
                i++;
            if (i < eigenValues.Length)
            {
                DeformedState deformedState = new FrameDeformedState();
                deformedState.AssignToModel(model);
                int counter = 0;
                int nodesCount = model.GetNodesCount();
                double dx, dy, da;
                for (int j = 0; j < nodesCount; j++)
                {
                    Node node = model.GetNodeBySystemIndex(j);
                    if (!node.Support.Sx)
                        dx = eigenVectors[counter++, i];
                    else
                        dx = 0;
                    if (!node.Support.Sy)
                        dy = eigenVectors[counter++, i];
                    else
                        dy = 0;
                    if (!node.Support.Sr)
                        da = eigenVectors[counter++, i];
                    else
                        da = 0;
                    NodeDisplacement nodeDisplacement = new NodeDisplacement(node, dx, dy, da);
                    deformedState.Add(j, nodeDisplacement);
                }

                return deformedState;
            }
            else
                return null;
        }

        public override List<double> GetEigenValuesList()
        {
            if (eigenValues == null)
                Analyse();
            List<double> result = new List<double>();
            for (int i = 0; i < eigenValues.Length; i++)
                result.Add(eigenValues[i]);
            return result;
        }

        public override List<double> GetEigenVecsListForEigenValue(double eigenValue)
        {
            if (eigenValues == null)
                Analyse();

            int i = 0;
            while (i < eigenValues.Length && !Number.AlmostEqual(eigenValue, eigenValues[i]))
                i++;
            List<double> result = new List<double>();
            if (i < eigenValues.Length)
            {
                for (int j = 0; j < eigenVectors.RowCount; j++)
                    result.Add(eigenVectors[j, i]);
            }
            return result;
        }

        private ISimpleMatrix BarTransformationMatrix(Bar bar, bool transposed)
        {
            double[][] tm = new double[6][];
            double c = bar.Cos;
            double s = bar.Sin;
            if (!transposed)
            {
                tm[0] = new double[6] { c, -s,  0, 0,  0,  0 };
                tm[1] = new double[6] { s,  c,  0, 0,  0,  0 };
                tm[2] = new double[6] { 0,  0,  1, 0,  0,  0 };
                tm[3] = new double[6] { 0,  0,  0, c, -s,  0 };
                tm[4] = new double[6] { 0,  0,  0, s,  c,  0 };
                tm[5] = new double[6] { 0,  0,  0, 0,  0,  1 };
            }
            else
            {
                tm[0] = new double[6] {  c, s, 0,  0, 0, 0 };
                tm[1] = new double[6] { -s, c, 0,  0, 0, 0 };
                tm[2] = new double[6] {  0, 0, 1,  0, 0, 0 };
                tm[3] = new double[6] {  0, 0, 0,  c, s, 0 };
                tm[4] = new double[6] {  0, 0, 0, -s, c, 0 };
                tm[5] = new double[6] {  0, 0, 0,  0, 0, 1 };
            }
            return new SimpleMatrix(tm);
        }

        private ISimpleMatrix BarGlobalStiffnessMatrix(Bar bar)
        {
            ISimpleMatrix result;

            result = MxOp.Product(BarTransformationMatrix(bar, false), BarLocalStiffnessMatrix(bar));
            result = MxOp.Product(result, BarTransformationMatrix(bar, true));

            return result;
        }

        private ISimpleMatrix BarGlobalMassMatrix(Bar bar)
        {
            ISimpleMatrix result;

            result = MxOp.Product(BarTransformationMatrix(bar, false), BarLocalMassMatrix(bar));
            result = MxOp.Product(result, BarTransformationMatrix(bar, true));

            return result;
        }

        private ISimpleMatrix BarGlobalMassMatrixWithRotationInertia(Bar bar)
        {
            ISimpleMatrix result;

            result = MxOp.Product(BarTransformationMatrix(bar, false), BarLocalMassMatrixWithRotationInertia(bar));
            result = MxOp.Product(result, BarTransformationMatrix(bar, true));

            return result;
        }

        private ISimpleMatrix BarLocalStiffnessMatrix(Bar bar)
        {
            double[][] lsm = new double[6][];
            double ea = bar.Phys.E * bar.Phys.A / bar.Length;
            double ei = bar.Phys.E * bar.Phys.I;
            double l1 = 1 / bar.Length;
            double l2 = l1 * l1;
            double l3 = l2 * l1;

            lsm[0] = new double[6] { ea, 0, 0, -ea, 0, 0 };
            lsm[1] = new double[6] { 0, 12 * ei * l3, 6 * ei * l2, 0, -12 * ei * l3, 6 * ei * l2 };
            lsm[2] = new double[6] { 0, 6 * ei * l2, 4 * ei * l1, 0, -6 * ei * l2, 2 * ei * l1 };
            lsm[3] = new double[6] { -ea, 0, 0, ea, 0, 0 };
            lsm[4] = new double[6] { 0, -12 * ei * l3, -6 * ei * l2, 0, 12 * ei * l3, -6 * ei * l2 };
            lsm[5] = new double[6] { 0, 6 * ei * l2, 2 * ei * l1, 0, -6 * ei * l2, 4 * ei * l1 };

            return new SimpleMatrix(lsm);
        }

        private ISimpleMatrix BarLocalMassMatrixWithRotationInertia(Bar bar)
        {
            double[][] lmm = new double[6][];
            double L = bar.Length;
            double m2 = 0.1 * bar.Phys.Density * bar.Phys.I;

            ISimpleMatrix result = this.BarLocalMassMatrix(bar);

            result.AddElement(1, 1, 6 * m2 / (5 * L));
            result.AddElement(1, 2, m2 / 10);
            result.AddElement(1, 4, -6 * m2 / (5 * L));
            result.AddElement(1, 5, m2 / 10);

            result.AddElement(2, 1, m2 / 10);
            result.AddElement(2, 2, 2 * L * m2 / 15);
            result.AddElement(2, 4, -m2 / 10);
            result.AddElement(2, 5, -L * m2 / 30);

            result.AddElement(4, 1, -6 * m2 / (5 * L));
            result.AddElement(4, 2, -m2 / 10);
            result.AddElement(4, 4, 6 * m2 / (5 * L));
            result.AddElement(4, 5, -m2 / 10);

            result.AddElement(5, 1, m2 / 10);
            result.AddElement(5, 2, -L * m2 / 30);
            result.AddElement(5, 4, -m2 / 10);
            result.AddElement(5, 5, 2 * L * m2 / 15);

            return new SimpleMatrix(result);
        }

        private ISimpleMatrix BarLocalMassMatrix(Bar bar)
        {
            double[][] lmm = new double[6][];
            double L = bar.Length;
            double dal = 0.1 * bar.Phys.Density * bar.Phys.A * L / 420;
            double dall = 0.1 * bar.Phys.Density * bar.Phys.A * L * L / 420;

            lmm[0] = new double[6] { 140 * dal, 0, 0, 70 * dal, 0, 0 };
            lmm[1] = new double[6] { 0, 156 * dal, 22 * dall, 0, 54 * dal, -13 * dall };
            lmm[2] = new double[6] { 0, 22 * dall, 4 * dall * L, 0, 13 * dall, -3 * dall * L };
            lmm[3] = new double[6] { 70 * dal, 0, 0, 140 * dal, 0, 0 };
            lmm[4] = new double[6] { 0, 54 * dal, 13 * dall, 0, 156 * dal, -22 * dall };
            lmm[5] = new double[6] { 0, -13 * dall, -3 * dall * L, 0, -22 * dall, 4 * dall * L };

            return new SimpleMatrix(lmm);
        }

        public override List<double> GetFullEigenVecsListForEigenValue(double eigenValue)
        {
            throw new NotImplementedException();
        }
    }
}
